#include "PushTracker.h"
#include "LPC214x.h"
#include <stdlib.h>

extern "C"
{
    #include "main.h"
    #include "ADXL345.h"
}

const float B[5] = {0.00370094327515829,0,-0.00740188655031657,0,0.00370094327515829};	// matlab bandpass filter : butter(2,[1/200 1/8])
const float A[5] = {1,-3.81639890322756,5.46866457896694,-3.48775379557443,0.835492729504173};

float z1=0.0;
float z2=0.0;
float z3=0.0;
float z4=0.0;

PushTracker::PushTracker()
{
    initialize = 0;
    Pushes = 0;
    speed = 0;
    distance = 0;
    z1=0.0;
    z2=0.0;
    z3=0.0;
    z4=0.0;
    min_thresh = 5.0;
    thresh = min_thresh;
    diff_thresh = 2.5;
    min_mask = 0;
    max_mask = 0;
    
    for (i=0;i<VAL_BUFFER_SIZE;i++)
    {
            min_times[i]=0;
            min_vals[i]=0;
            max_times[i]=0;
            max_vals[i]=0;
    }	
    
    for (i=0;i<RAF_BUFFER_SIZE;i++)	
    {
            RAF_diff[i]=0;
            push_times[i]=0;
    }
}

PushTracker::PushTracker(float m_thresh, float d_thresh)
{
    initialize = 0;
    Pushes = 0;
    speed = 0;
    distance = 0;
    z1=0.0;
    z2=0.0;
    z3=0.0;
    z4=0.0;
    min_thresh = m_thresh;
    thresh = min_thresh;
    diff_thresh = d_thresh;
    min_mask = 0;
    max_mask = 0;
    
    for (i=0;i<VAL_BUFFER_SIZE;i++)
    {
            min_times[i]=0;
            min_vals[i]=0;
            max_times[i]=0;
            max_vals[i]=0;
    }	
    
    for (i=0;i<RAF_BUFFER_SIZE;i++)	
    {
            RAF_diff[i]=0;
            push_times[i]=0;
    }
}

PushTracker::~PushTracker()
{
}

bool PushTracker::UpdateBuffers(unsigned long time)
{
        PushFlag = false;
        if (initialize<RAW_BUFFER_SIZE)
        {                
                raw_buffer[initialize] = accelY();
                time_buffer[initialize] = time;
                
                filtered_buffer[initialize] = (B[0]*raw_buffer[initialize])+z1;
                z1 = (B[1]*raw_buffer[initialize])+z2-(A[1]*filtered_buffer[initialize]);
                z2 = (B[2]*raw_buffer[initialize])+z3-(A[2]*filtered_buffer[initialize]);
                z3 = (B[3]*raw_buffer[initialize])+z4-(A[3]*filtered_buffer[initialize]);
                z4 = (B[4]*raw_buffer[initialize])-(A[4]*filtered_buffer[initialize]);
                
                initialize++;
        }
        else
        {
                minima();
                maxima();
                
                if ((max_mask&0x01) == 0)		// to see if there is a new max
                {
                        if ((min_mask&0x01)==0)
                        {
                                if (max_times[VAL_BUFFER_SIZE-1]>min_times[VAL_BUFFER_SIZE-1]&& (max_times[VAL_BUFFER_SIZE-1]-min_times[VAL_BUFFER_SIZE-1])<2000)		// && (max_times[VAL_BUFFER_SIZE-1]-min_times[VAL_BUFFER_SIZE-1])<1500
                                {
                                        min_mask |= 0x01;
                                        max_mask |= 0x01;
                                        for (i=0;i<RAF_BUFFER_SIZE-1;i++) RAF_diff[i] = RAF_diff[i+1];
                                        RAF_diff[RAF_BUFFER_SIZE-1] = max_vals[VAL_BUFFER_SIZE-1]-min_vals[VAL_BUFFER_SIZE-1];
                                        
                                        if (RAF_diff[RAF_BUFFER_SIZE-1]>thresh) 
                                        {
                                                if ((min_times[VAL_BUFFER_SIZE-1]-push_times[RAF_BUFFER_SIZE-1])>300)
                                                {
                                                        for (i=0;i<(RAF_BUFFER_SIZE-1);i++) push_times[i]=push_times[i+1];
                                                        push_times[RAF_BUFFER_SIZE-1] = min_times[VAL_BUFFER_SIZE-1];
                                                        Pushes++;
                                                        PushFlag=TRUE;
                                                }
                                        }
                                        mean=0.0;
                                        i=0;
                                        while (RAF_diff[i]) i++;
                                        if (i==RAF_BUFFER_SIZE)
                                        {
                                                for (i=0;i<RAF_BUFFER_SIZE;i++)	mean+=RAF_diff[i];
                                                mean = mean/RAF_BUFFER_SIZE;
                                                thresh=mean*(float)1/2;
                                                if (thresh<min_thresh)
                                                {
                                                        thresh=min_thresh;
                                                }
                                        }
                                }
                                else 
                                {
                                        if (max_times[VAL_BUFFER_SIZE-1]>min_times[VAL_BUFFER_SIZE-2] && min_mask&0x02 == 0)
                                        {
                                                min_mask |= 0x02;
                                                max_mask |= 0x01;
                                                for (i=0;i<RAF_BUFFER_SIZE-1;i++) RAF_diff[i] = RAF_diff[i+1];
                                                RAF_diff[RAF_BUFFER_SIZE-1] = max_vals[VAL_BUFFER_SIZE-1]-min_vals[VAL_BUFFER_SIZE-2];
                                                
                                                if (RAF_diff[RAF_BUFFER_SIZE-1]>thresh) 
                                                {
                                                        if ((min_times[VAL_BUFFER_SIZE-2]-push_times[RAF_BUFFER_SIZE-1])>300)
                                                        {
                                                                for (i=0;i<(RAF_BUFFER_SIZE-1);i++) push_times[i]=push_times[i+1];
                                                                push_times[RAF_BUFFER_SIZE-1] = min_times[VAL_BUFFER_SIZE-2];
                                                                Pushes++;
                                                                PushFlag=TRUE;
                                                        }
                                                }
                                                //mean=0.0;
                                                //i=0;
                                                //while (RAF_diff[i]) i++;
                                                //if (i==RAF_BUFFER_SIZE)
                                                //{
                                                //	for (i=0;i<RAF_BUFFER_SIZE;i++)	mean+=RAF_diff[i];
                                                //	mean = mean/(float)RAF_BUFFER_SIZE;
                                                //	thresh=mean*(float)1/2;
                                                //	if (thresh<min_thresh)
                                                //	{
                                                //		thresh=min_thresh;
                                                //	}
                                                //}
                                        }
                                        else max_mask |= 0x01;
                                }
                        }
                }
                
                for (i=0;i<RAW_BUFFER_SIZE-1;i++)
                {
                        time_buffer[i] = time_buffer[i+1];
                        raw_buffer[i] = raw_buffer[i+1];
                        filtered_buffer[i] = filtered_buffer[i+1];
                }
                raw_buffer[RAW_BUFFER_SIZE-1] = accelY();
                time_buffer[RAW_BUFFER_SIZE-1] = time;
                
                filtered_buffer[RAW_BUFFER_SIZE-1] = (B[0]*raw_buffer[RAW_BUFFER_SIZE-1])+z1;
                z1 = (B[1]*raw_buffer[RAW_BUFFER_SIZE-1])+z2-(A[1]*filtered_buffer[RAW_BUFFER_SIZE-1]);
                z2 = (B[2]*raw_buffer[RAW_BUFFER_SIZE-1])+z3-(A[2]*filtered_buffer[RAW_BUFFER_SIZE-1]);
                z3 = (B[3]*raw_buffer[RAW_BUFFER_SIZE-1])+z4-(A[3]*filtered_buffer[RAW_BUFFER_SIZE-1]);
                z4 = (B[4]*raw_buffer[RAW_BUFFER_SIZE-1])-(A[4]*filtered_buffer[RAW_BUFFER_SIZE-1]);
                
                if (PushFlag==TRUE) return true;
        }
        return false;
}

// New minfinder functional generator!
// This looks for minima by seraching and indexing when you have a minima in
// the middle of a set of points, not at either end

void PushTracker::minima(void)
{ 
	int i;
	int _index = 0;
	for (i=1; i<=(RAW_BUFFER_SIZE-1); i++) _index = (filtered_buffer[_index]<filtered_buffer[i]) ? _index : i;
	
	if (_index>0 && _index<(RAW_BUFFER_SIZE-1))
	{
		if (time_buffer[_index] > min_times[VAL_BUFFER_SIZE-1])		//time_buffer[_index] > min_times[VAL_BUFFER_SIZE-1] && 
		{
			if (max_times[VAL_BUFFER_SIZE-1] > min_times[VAL_BUFFER_SIZE-1])
			{
				if (abs(max_vals[VAL_BUFFER_SIZE-1]-filtered_buffer[_index])>diff_thresh && (time_buffer[_index]-max_times[VAL_BUFFER_SIZE-1])>300)
				{
					for (i=0;i<(VAL_BUFFER_SIZE-1);i++)				//if (abs(max_vals[VAL_BUFFER_SIZE-1]-filtered_buffer[_index])>diff_thresh)		// to make sure that the min is different enough from the last ma
					{
						min_times[i] = min_times[i+1];
						min_vals[i] = min_vals[i+1];
					}
					min_mask = min_mask << 1;
					min_times[VAL_BUFFER_SIZE-1] = time_buffer[_index];
					min_vals[VAL_BUFFER_SIZE-1] = filtered_buffer[_index];
				}
			}
			else
			{
				if (min_vals[VAL_BUFFER_SIZE-1]<=filtered_buffer[_index]) return;
				min_mask &= ~(0x01);			// sets bit 0 to 0
				min_times[VAL_BUFFER_SIZE-1] = time_buffer[_index];
				min_vals[VAL_BUFFER_SIZE-1] = filtered_buffer[_index];
				//max_mask &= ~(0x01);			// to check the last min with the new max
			}
		}
	}
}

// New maxfinder functional generator!
// This looks for maxes by seraching and indexing when you have a mmaxima in
// the middle of a set of points, not at either end

void PushTracker::maxima(void)
{ 
	int i;
	int _index = 0;
	for (i=1; i<=(RAW_BUFFER_SIZE-1); i++) _index = (filtered_buffer[_index]>filtered_buffer[i]) ? _index : i; 
	
	if (_index>0 && _index<(RAW_BUFFER_SIZE-1))
	{
		if (time_buffer[_index] > max_times[VAL_BUFFER_SIZE-1])
		{
			if (min_times[VAL_BUFFER_SIZE-1] > max_times[VAL_BUFFER_SIZE-1])
			{
				if (abs(min_vals[VAL_BUFFER_SIZE-1]-filtered_buffer[_index])>diff_thresh && (time_buffer[_index]-min_times[VAL_BUFFER_SIZE-1])>300)
				{
					for (i=0;i<(VAL_BUFFER_SIZE-1);i++)
					{
						max_times[i] = max_times[i+1];
						max_vals[i] = max_vals[i+1];
					}
					max_mask = max_mask << 1;
					max_times[VAL_BUFFER_SIZE-1] = time_buffer[_index];
					max_vals[VAL_BUFFER_SIZE-1] = filtered_buffer[_index];
				}
			}
			else
			{
				if (max_vals[VAL_BUFFER_SIZE-1]>=filtered_buffer[_index]) return;
				max_mask &= ~(0x01);			// sets bit 0 to 0
				max_times[VAL_BUFFER_SIZE-1] = time_buffer[_index];
				max_vals[VAL_BUFFER_SIZE-1] = filtered_buffer[_index];
				min_mask &= ~(0x01);			// to check the last min with the new max
			}
		}
	}
}

